// (c) Copyright 2014 Stephen Gold <sgold@sonic.net>
// Distributed under the terms of the GNU General Public License

/*
 This file is part of the Bats Game.

 The Bats Game is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by the
 Free Software Foundation, either version 3 of the License, or (at your
 option) any later version.

 The Bats Game is distributed in the hope that it will be useful, but
 WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 for more details.

 You should have received a copy of the GNU General Public License
 along with the Bats Game.  If not, see <http://www.gnu.org/licenses/>.
 */
package bats.unit;

import com.jme3.animation.Bone;
import com.jme3.bullet.PhysicsSpace;
import com.jme3.bullet.collision.shapes.CapsuleCollisionShape;
import com.jme3.bullet.collision.shapes.CompoundCollisionShape;
import com.jme3.bullet.collision.shapes.SphereCollisionShape;
import com.jme3.bullet.control.RigidBodyControl;
import com.jme3.bullet.joints.ConeJoint;
import com.jme3.math.Matrix3f;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import com.jme3.scene.Node;
import java.util.Map;
import java.util.logging.Logger;
import jme3utilities.MySkeleton;
import jme3utilities.MySpatial;
import jme3utilities.math.MyVolume;

/**
 * A bone which has physical solidity. Its shape consists of a spherical "head"
 * and a capsule-shaped "shaft". Each instance is contained in exactly one
 * collection of such bones.
 *
 * @author Stephen Gold <sgold@sonic.net>
 */
class SolidBone {
    // *************************************************************************
    // constants

    /**
     * message logger for this class
     */
    final private static Logger logger =
            Logger.getLogger(SolidBone.class.getName());
    // *************************************************************************
    // fields
    /**
     * corresponding bone in the skeleton: set by constructor
     */
    final private Bone bone;
    /**
     * first child of the corresponding bone in the skeleton: set by constructor
     */
    final private Bone child;
    /**
     * radius of this bone's head (in model units, &ge;0)
     */
    final private float headRadius;
    /**
     * radius of this bone's shaft (in model units, &ge;0)
     */
    final private float shaftRadius;
    /**
     * which collection contains this bone: set by constructor
     */
    final private Map<String, SolidBone> collection;
    /**
     * scene-graph node for the animated model: set by constructor
     */
    final private Node model;
    /**
     * scene-graph node for this bone's physics control (or null if the control
     * hasn't been created yet)
     */
    private Node objectNode = null;
    /**
     * orientation of this bone relative to its object (or null of the object
     * hasn't been created yet)
     */
    private Quaternion boneInObject = null;
    /**
     * physics control for this bone (or null if the control hasn't been created
     * yet)
     */
    private RigidBodyControl rigidBody = null;
    /**
     * offset of the bone's tail in the physics object (or null if the object
     * hasn't been created yet)
     */
    private Vector3f objectPivot = null;
    // *************************************************************************
    // constructors

    /**
     * Instantiate a solid bone in the specified collection.
     *
     * @param headRadius (in model units, &ge;0)
     * @param shaftRadius (in model units, &ge;0)
     * @param collection (not null)
     * @param model animated node which contains this bone (not null)
     * @param boneName (not null)
     */
    SolidBone(float headRadius, float shaftRadius,
            Map<String, SolidBone> collection, Node model, String boneName) {
        assert headRadius >= 0f : headRadius;
        assert shaftRadius >= 0f : shaftRadius;
        assert collection != null;
        assert model != null;
        assert boneName != null;

        this.bone = MySkeleton.getBone(model, boneName);
        this.headRadius = headRadius;
        this.shaftRadius = shaftRadius;
        this.collection = collection;
        this.model = model;

        child = bone.getChildren().get(0);
        assert child != null : bone.getName();
    }
    // *************************************************************************
    // new methods exposed

    /**
     * Augment an existing collision shape with this bone's shape.
     *
     * @param result collision shape to augment (not null)
     * @param modelCenter model coordinates of the shape's center-of-mass (not
     * null)
     */
    void addTo(CompoundCollisionShape result, Vector3f modelCenter) {
        assert result != null;
        assert modelCenter != null;

        addHeadTo(result, modelCenter);
        addShaftTo(result, modelCenter);
    }

    /**
     * Look up this bone's parent.
     *
     * @return the pre-existing instance (or null if none or if the parent bone
     * is not solid)
     */
    SolidBone getSolidParent() {
        Bone parent = bone.getParent();
        String parentName = parent.getName();
        SolidBone result = collection.get(parentName);
        return result;
    }

    /**
     * Create a physics object for this bone, then do the same for all of the
     * bone's solid descendents. The object will be centered on the bone's shaft
     * and joined to a specified parent by means of a joint. NOTE: recursive!
     *
     * @param parentBody existing object to join (not null)
     * @param density in kilograms per cubic world unit (&gt;0)
     * @param parentInWorld physicsOrientation of the parent bone in world
     * coordinates (not null)
     */
    void joinTo(RigidBodyControl parentBody, float density,
            Quaternion parentInWorld) {
        assert parentBody != null;
        assert density > 0f : density;
        assert parentInWorld != null;
        assert rigidBody == null : rigidBody;
        /*
         * Generate a collision shape for this bone.
         */
        Vector3f boneCenter = shaft();
        CompoundCollisionShape boneShape = new CompoundCollisionShape();
        addTo(boneShape, boneCenter);
        /*
         * Create an "object node" with a dynamic rigid body control.
         */
        String boneName = bone.getName();
        String nodeName = String.format("%s %s", model.getName(), boneName);
        objectNode = new Node(nodeName);
        model.attachChild(objectNode);
        /*
         * Initialize the object's mass, physics space, and position.
         */
        float mass = density * MyVolume.volume(boneShape);
        assert mass > 0f : mass;
        rigidBody = new RigidBodyControl(boneShape, mass);
        PhysicsSpace physicsSpace = parentBody.getPhysicsSpace();
        physicsSpace.add(rigidBody);
        objectNode.addControl(rigidBody);
        Vector3f worldCenter = model.localToWorld(boneCenter, null);
        rigidBody.setPhysicsLocation(worldCenter);
        rigidBody.setPhysicsRotation(MySpatial.getWorldOrientation(model));
        /*
         * Using a cone joint at this bone's tail, attach the new object
         * to the parent object.
         */
        boneInObject = bone.getModelSpaceRotation().clone();

        Vector3f parentCenter = parentBody.getPhysicsLocation();
        parentCenter = model.worldToLocal(parentCenter, null);
        Vector3f parentPivot = tail().subtract(parentCenter);
        float modelScale = MySpatial.getUniformScale(model);
        parentPivot.multLocal(modelScale);
        objectPivot = shaftOffset().multLocal(-modelScale);
        Quaternion boneBasis = parentInWorld; // TODO rotate to bias
        Matrix3f basisMatrix = boneBasis.toRotationMatrix();

        ConeJoint joint = new ConeJoint(parentBody, rigidBody, parentPivot,
                objectPivot, basisMatrix, basisMatrix);
        float swingSpan1 = 1f; // radians
        float swingSpan2 = 1f; // radians
        float twistSpan = 0.1f; // radians
        joint.setLimit(swingSpan1, swingSpan2, twistSpan);
        physicsSpace.add(joint);

        joinToChildren(density);
    }

    /**
     * Get the bone's orientation in world per the physics.
     *
     * @return a new object
     */
    Quaternion physicsOrientation() {
        Quaternion result = rigidBody.getPhysicsRotation().mult(boneInObject);
        return result;
    }

    /**
     * Get the bone's tail location in world per the physics.
     *
     * @return a new vector
     */
    Vector3f physicsTail() {
        Vector3f offset = rigidBody.getPhysicsRotation().mult(objectPivot);
        Vector3f result = rigidBody.getPhysicsLocation().add(offset);

        return result;
    }
    // *************************************************************************
    // private methods

    /**
     * Augment an existing compound collision shape with the shape of this
     * bone's head.
     *
     * @param result shape to augment (not null)
     * @param modelCenter model coordinates of the shape's center-of-mass (not
     * null)
     */
    private void addHeadTo(CompoundCollisionShape result,
            Vector3f modelCenter) {
        assert result != null;
        assert modelCenter != null;

        SphereCollisionShape shape = headShape();
        if (shape == null) {
            return;
        }

        Vector3f offset = head();
        offset.subtractLocal(modelCenter);
        float modelScale = MySpatial.getUniformScale(model);
        offset.multLocal(modelScale);
        result.addChildShape(shape, offset);
    }

    /**
     * Augment an existing compound collision shape with the shape of this
     * bone's shaft.
     *
     * @param result shape to augment (not null)
     * @param modelCenter model coordinates of the shape's center-of-mass (not
     * null)
     */
    private void addShaftTo(CompoundCollisionShape result,
            Vector3f modelCenter) {
        assert result != null;
        assert modelCenter != null;

        CapsuleCollisionShape shape = shaftShape();
        if (shape == null) {
            return;
        }

        Vector3f offset = shaft();

        offset.subtractLocal(modelCenter);
        float modelScale = MySpatial.getUniformScale(model);
        offset.multLocal(modelScale);

        Quaternion boneInModel = bone.getModelSpaceRotation();
        Matrix3f rotationMatrix = boneInModel.toRotationMatrix();

        result.addChildShape(shape, offset, rotationMatrix);
    }

    /**
     * Calculate the location of the center of this bone's head in model space:
     * where this bone attaches to its children.
     *
     * @return a new vector
     */
    private Vector3f head() {
        Vector3f result = child.getModelSpacePosition();
        /*
         * getModelSpacePosition() returns an internal vector, so clone it.
         */
        return result.clone();
    }

    /**
     * Calculate the model-space offset of this bone's head relative to its
     * tail.
     *
     * @return a new vector (in model units)
     */
    private Vector3f headOffset() {
        Vector3f headLocation = head();
        Vector3f tailLocation = tail();
        Vector3f result = headLocation.subtract(tailLocation);
        return result;
    }

    /**
     * Generate a collision shape for this bone's head.
     *
     * @return a new instance (or null for none)
     */
    private SphereCollisionShape headShape() {
        if (headRadius == 0f) {
            return null;
        }
        /*
         * Scale the radius from model units to world units.
         */
        float modelScale = MySpatial.getUniformScale(model);
        float radius = headRadius * modelScale;

        SphereCollisionShape shape = new SphereCollisionShape(radius);
        return shape;
    }

    /**
     * Join this bone to its solid children, if any. NOTE: recursive!
     *
     * @param density in kilograms per cubic world unit (&gt;0)
     */
    private void joinToChildren(float density) {
        assert density > 0f : density;

        Quaternion boneInWorld = MySkeleton.worldOrientation(model, bone);
        for (Bone kid : bone.getChildren()) {
            String kidName = kid.getName();
            SolidBone solidChild = collection.get(kidName);
            if (solidChild != null) {
                solidChild.joinTo(rigidBody, density, boneInWorld);
            }
        }
    }

    /**
     * Calculate the location of the center of this bone's shaft in model space,
     * which is also its center of mass.
     *
     * @return a new vector (in model units)
     */
    private Vector3f shaft() {
        Vector3f tailLocation = tail();
        Vector3f centerOffset = shaftOffset();
        Vector3f result = tailLocation.add(centerOffset);
        return result;
    }

    /**
     * Calculate the offset of the center of this bone's shaft relative to the
     * bone's tail in model space.
     *
     * @return a new vector (in model units)
     */
    private Vector3f shaftOffset() {
        Vector3f offset = headOffset();
        float boneLength = offset.length();
        if (boneLength == 0f) {
            return offset;
        }
        float tailRadius = tailRadius();
        float shaftLength = boneLength - headRadius - tailRadius;
        float midDistance = shaftLength / 2f + tailRadius;
        float ratio = midDistance / boneLength;
        offset.multLocal(ratio);

        return offset;
    }

    /**
     * Generate an collision shape for this bone's shaft, oriented to the Y
     * axis.
     *
     * @param modelScale to be applied to dimensions (&gt;0)
     * @return a new instance (or null for none)
     */
    private CapsuleCollisionShape shaftShape() {
        if (shaftRadius == 0f) {
            return null;
        }
        /*
         * Calculate shaft dimensions in model units.
         */
        Vector3f headOffset = headOffset();
        float shaftLength = headOffset.length() - headRadius - tailRadius();
        float capsuleLength = shaftLength - 2f * shaftRadius;
        if (capsuleLength <= 0f) {
            return null;
        }
        /*
         * Scale the dimensions from model units to world units.
         */
        float modelScale = MySpatial.getUniformScale(model);
        float radius = shaftRadius * modelScale;
        float length = capsuleLength * modelScale;

        CapsuleCollisionShape result =
                new CapsuleCollisionShape(radius, length);
        return result;
    }

    /**
     * Calculate the location of the center of this bone's tail in model space,
     * which is also where the bone attaches to its parent.
     *
     * @return a new vector (in model units)
     */
    private Vector3f tail() {
        Vector3f result = bone.getModelSpacePosition();
        /*
         * getModelSpacePosition() returns an internal vector, so clone it.
         */
        return result.clone();
    }

    /**
     * Look up the radius of this bone's tail, which is its parent's head, or
     * zero if it has no solid parent.
     *
     * @return radius (in model units, &ge;0)
     */
    private float tailRadius() {
        SolidBone solidParent = getSolidParent();
        if (solidParent == null) {
            return 0f;
        }
        float result = solidParent.headRadius;

        assert result >= 0f : result;
        return result;
    }
}